#!/usr/bin/env python

import rospy
from nav_msgs.msg import Odometry
from trajectory_msgs.msg import MultiDOFJointTrajectory

from geometry_msgs.msg import PoseStamped

def vrpn_callback(vrpn_msg):
    #pose_msg=PoseStamped()
    pose_msg=vrpn_msg
    new_odom_msg = Odometry()
    new_odom_msg.header=pose_msg.header
    new_odom_msg.pose.pose=pose_msg.pose
    new_odom_msg.child_frame_id="body"
    
    pub.publish(new_odom_msg)
    print(new_odom_msg)

def odometry_callback(odom_msg):
    # Create a new Odometry message
    new_odom_msg = Odometry()

    # Copy the contents of the received message
    new_odom_msg.header = odom_msg.header
    new_odom_msg.child_frame_id = odom_msg.child_frame_id
    new_odom_msg.pose = odom_msg.pose
    new_odom_msg.twist = odom_msg.twist

    # Publish the modified Odometry message to /hummingbird/ground_truth/odometry topic
    pub.publish(new_odom_msg)
    
    
    
def traj_callback(traj_msg):
    #msg=MultiDOFJointTrajectory()
    msg=traj_msg
    first_traj_point=msg.points[0]
    vel=first_traj_point.velocities[0]
    print(vel)

if __name__ == '__main__':
    rospy.init_node('odometry_converter')
    
    # Create a subscriber for the /bebop2/ground_truth/odometry topic
    rospy.Subscriber('/bebop2/ground_truth/odometry', Odometry, odometry_callback)
    #rospy.Subscriber('/hummingbird/predicted/mdj_trajectory',MultiDOFJointTrajectory,traj_callback)
    #rospy.Subscriber('/vrpn_client_node/Tello4/pose',PoseStamped,vrpn_callback)
    
    # Create a publisher for the /hummingbird/ground_truth/odometry topic
    pub = rospy.Publisher('/hummingbird/ground_truth/odometry', Odometry, queue_size=10)
    
    rospy.spin()